Globally consistent 3D mapping with scan matching

A globally consistent solution to the simultaneous localization and mapping (SLAM) problem in 2D with three degrees of freedom (DoF) poses was presented by Lu and Milios [20]. To create maps suitable for natural environments it is however necessary to consider the 6 DoF case, namely the three Cartesian coordinates and the roll, pitch and yaw angles. This article describes the extension of the propsed algorithm to deal with these additional DoFs and the resulting non-linearities. Simplifications using Taylor expansion and Cholesky decomposition yield a fast application that handles the massive amount of 3D data and the computational requirements due to the 6 DoF. Our Experiments demonstrate the functionality of estimating the exact poses and their covariances in all 6 DoF, leading to a globally consistent map. The correspondences between scans are found automatically by use of a simple distance heuristic.